/**
  ==============================================================================
                      ##### How to use this driver #####
  ==============================================================================
    @note
		-# WHLENGTH 为轮轴距离
    @warning
  ******************************************************************************
  */
#include "App_Chassis.h"

#define WHLENGTH 0.3f

extern CAN_HandleTypeDef hcan1;

Motor_820R ChassisMotor1(1);
Motor_820R ChassisMotor2(2);
Motor_820R ChassisMotor3(3);

MotorSpeedCtrl<myPID> ChassisMotorCtrl1(&ChassisMotor1);
MotorSpeedCtrl<myPID> ChassisMotorCtrl2(&ChassisMotor2);
MotorSpeedCtrl<myPID> ChassisMotorCtrl3(&ChassisMotor3);

GlobalC GlobalCurrent,GlobalTarget;					//世界坐标系速度
ChassisC ChassisCurrent,ChassisTarget; 			//底盘解算轮子速度
AngleY yaw;																	//底盘在世界坐标系中角度和sincos



void Chassis_Init(void)
{
	ChassisMotorCtrl1.SpeedPID.SetPIDParam(5,15,0,20000,27500);  
	
	ChassisMotorCtrl2.SpeedPID.SetPIDParam(6,20,0,20000,27500);  	

	ChassisMotorCtrl3.SpeedPID.SetPIDParam(6,10,0,20000,27500);  
	
	yaw.sin_theta = 0.5f;
  yaw.cos_theta = 0.866025f;
}


void Chassis_AngleCal(AngleY *x)
{
	//q15_t angle,sina,cosa;
	//angle = x->yaw*(1<<15);
	//sina = arm_sin_q15(angle);
	//cosa = arm_cos_q15(angle);
	//x->sin_theta = sina /(1<<15);
	//x->cos_theta = cosa /(1<<15);
//	x->sin_theta = 0.5f;
//	x->cos_theta = 0.866025f;
}

void Chassis_Cal(GlobalC *gc, ChassisC *cc, AngleY *x)
{
	cc->Wheel1 = (WHLENGTH * gc->az) - (x->sin_theta * gc->vx)  - (x->cos_theta * gc->vy);
	cc->Wheel2 = (WHLENGTH * gc->az) + gc->vx;
	cc->Wheel3 = (WHLENGTH * gc->az) - (x->sin_theta * gc->vx)  + (x->cos_theta * gc->vy);
	return ;
}

void Chassis_arCal(ChassisC *cc,GlobalC *gc, AngleY *x)
{
	gc->vx = -1.0f/(2.0f*x->sin_theta+1.0f)*(cc->Wheel1) + 1.0f/(x->sin_theta+1.0f)*(cc->Wheel2) - 1.0f/(2.0f*x->sin_theta+1.0f)*(cc->Wheel3);
	gc->vy = -1.0f/(2.0f*x->cos_theta)    *(cc->Wheel1) + 0.0f                                 + 1.0f/(2.0f*x->cos_theta)  *(cc->Wheel3);
	gc->az = 1.0f/(2.0f*(WHLENGTH+WHLENGTH*x->sin_theta))*(cc->Wheel1) + x->sin_theta/(x->sin_theta*WHLENGTH+WHLENGTH)*(cc->Wheel2) + 1.0f/(2.0f*(WHLENGTH*x->sin_theta+WHLENGTH))*(cc->Wheel3);
	return ;
}

uint32_t Chassis_Receive_HERO(uint8_t *data_buf,uint16_t length)
{
	for(uint8_t i = 0; i < length -13; i++)
	{
		if(data_buf[i] == 0xA5)
		{
			memcpy(&GlobalTarget.vx, &data_buf+1, sizeof(float));
			memcpy(&GlobalTarget.vy, &data_buf+5, sizeof(float));
			memcpy(&GlobalTarget.az, &data_buf+9, sizeof(float));
			//memcpy(&yaw.yaw, &data_buf+13, sizeof(float));
			Chassis_Cal(&GlobalTarget,&ChassisTarget,&yaw);
			
			ChassisMotorCtrl1.setTarget(ChassisTarget.Wheel1*440.94736f);
			ChassisMotorCtrl2.setTarget(ChassisTarget.Wheel2*440.94736f);
			ChassisMotorCtrl3.setTarget(ChassisTarget.Wheel3*440.94736f);
			
			i += 13;
		}
	} 
	return 0;
}


type_change temp[5];

void Chassis_Transmit_HERO(UART_HandleTypeDef* huart_x)
{
	ChassisCurrent.Wheel1 = ChassisMotor1.getSpeed()/440.94736f;
	ChassisCurrent.Wheel2 = ChassisMotor2.getSpeed()/440.94736f;
	ChassisCurrent.Wheel3 = ChassisMotor3.getSpeed()/440.94736f;
	Chassis_arCal(&ChassisCurrent,&GlobalCurrent,&yaw);
	
	
	
	//memcpy(&temp+1, &GlobalCurrent.vx, sizeof(float));
	//memcpy(&temp+5, &GlobalCurrent.vy, sizeof(float));
	//memcpy(&temp+9, &GlobalCurrent.az, sizeof(float));
	//memcpy(&temp+9, &GlobalCurrent.az, sizeof(float));
	
	//temp[13] = 0x5A;
//	memset(&temp+1, 0, sizeof(float));
//	memset(&temp+5, 0, sizeof(float));
//	memset(&temp+9, 0, sizeof(float));
	
	//temp.tail = (uint8_t)0x5A;
	temp[0].change_int = 0xAFAFAFAF;
	temp[1].change_float = GlobalCurrent.vx;
	temp[2].change_float = GlobalCurrent.vy;
	temp[3].change_float = GlobalCurrent.az;
	temp[4].change_int = 0xFAFAFAFA;
	
	HAL_UART_Transmit_DMA(huart_x,(uint8_t*)temp,20);
	return ;
}

void Chassis_Receive_Motor(CAN_RxBuffer* CRM)
{
	if(ChassisMotor1.CheckID(CRM->header.StdId))
    {
        ChassisMotor1.update(CRM->data);
    }
	if(ChassisMotor2.CheckID(CRM->header.StdId))
    {
        ChassisMotor2.update(CRM->data);
    }
	if(ChassisMotor3.CheckID(CRM->header.StdId))
    {
        ChassisMotor3.update(CRM->data);
    }
}


void Chassis_Adjust()
{
	
	ChassisMotorCtrl1.Adjust();
	ChassisMotorCtrl2.Adjust();
	ChassisMotorCtrl3.Adjust();
	
	Motor_820R ChassisMotors[3] = {ChassisMotor1,ChassisMotor2,ChassisMotor3};
	
	MotorMsgSend(&hcan1, ChassisMotors);
}

//void Chassis_World_Angle()
//{
//	if(!MPU_Get_Gyroscope(&MPU6050_IIC_PIN, &MPUData.gx, &MPUData.gy, &MPUData.gz))
//         {
//            MPUData.gx -= MPUData.gxoffset;
//            MPUData.gy -= MPUData.gyoffset;
//            MPUData.gz -= MPUData.gzoffset;
//         }
//				 
//	MPU_Get_Accelerometer(&MPU6050_IIC_PIN,&MPUData.ax,&MPUData.ay,&MPUData.az);
//				 
//	mpu_dmp_get_data(&MPUData.roll,&MPUData.pitch,&MPUData.yaw);
//}

